#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Encoder_Interface.h"
#include "TIM.h"
#include "PWM.h"
#include "Uart.h"
#include "DMA_UART1.h"
#include <stdio.h>
#include "PID.h"
#include "Rp.h"
#include "Key.h"
#include "I2C.h"
/*
    角度环电位器，能稳定倒立时的ADC输入值
*/
#define DEGREE_RP_CENTER 2023

static char buf[20];
uint16_t current_encoder_count;


/*
位置pid，外环pid
*/
PID_TypeDef location_pid = {
    .Kp=0.26,
    .Ki=0.02,
    /*我的技巧是先把D项阻尼调大，抑制抖动*/
    .Kd=2.72,
 
    .target = 0, // 由电位器控制
    
    .sum_upper_limit = 4080,
    .sum_lower_limit = -4080,
    
    .out_upper_limit = 100,
    .out_lower_limit = -100,
    
};

/*
角度pid，内环pid
*/
PID_TypeDef degree_pid = {
    .Kp=0.15,
    .Ki=0.02,
    .Kd=0.19,
    /* 角度传感器（电位器）的目标值
        正常来说应该是 4096/2=2048
    但是由于电位器装配工艺的问题导致实际值很可能上下浮动
        找到中点的方法：
    设置target值，烧录程序
    把摆杆向右偏，电机顺势向右旋转
    把摆杆向左偏，电机顺势向左旋转
    的target值大概率是电位器的中点
    */
    .target = DEGREE_RP_CENTER,
    
    .sum_upper_limit = 1000,
    .sum_lower_limit = -1000,
    
    .out_upper_limit = 100,
    .out_lower_limit = -100,
    
};

int main(void)
{
    
    RCC_HSEConfig(RCC_HSE_ON);
    
    while(RCC_WaitForHSEStartUp()==ERROR);
    
    RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);
    
    RCC_PLLCmd(ENABLE);
    
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
    
 
    
	/*模块初始化*/
    

	
    Key_Init();

    
    init_uart_with_dma();
    
    init_dma_for_uart1();
    

    I2C1_Init();
    
    OLED_Init();//OLED初始化
    
    
    init_tim();
    
    

    init_encoder_interface_in_tim3();
    
    init_my_pwm_in_tim2();
    
    start_pwm();
    
    initRp();
    

	while (1)
	{
        /*
        sprintf(buf,"K:%2.1f-%2.1f-%2.1f", degree_pid.Kp,degree_pid.Ki,degree_pid.Kd);
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"cur_enc:%d", current_encoder_count);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"tar:%3.1f", degree_pid.target);
        
        OLED_ShowString(3,1,buf);
            
        sprintf(buf,"cur:%3.1f-%3.1f",degree_pid.current, degree_pid.out);
        
        OLED_ShowString(4,1,buf);
        */
        uint8_t * str  = getBuffer();


        int length = sprintf((char *)str,"%4.2f,%4.2f,%4.2f,%f,%f,%f,%f\r\n",
           location_pid.Kp,
           location_pid.Ki,
           location_pid.Kd,
           location_pid.target,
           location_pid.current,
           location_pid.error,
           location_pid.out 
        );
        
        send(length);
        
	}
}
/**
    adc 的 dma 传输完成中断已被关闭
*/
void DMA1_Channel1_IRQHandler(void){
    if(DMA_GetITStatus(DMA1_IT_TC1)==SET){
        /*
        static char buf[20];
        
        
        sprintf(buf,"rp1:%d %d", get_rp_val(0),k1);
        OLED_ShowString(1,1,buf);
        
        sprintf(buf,"rp2:%d %d", get_rp_val(1),k2);
        OLED_ShowString(2,1,buf);
        
        sprintf(buf,"rp3:%d %d", get_rp_val(2),k3);
        OLED_ShowString(3,1,buf);
        
        sprintf(buf,"rp4:%d %d", get_rp_val(3),k4);
        OLED_ShowString(4,1,buf);
        */
        //uint16_t duty = get_rp_val(0)*2.44;//*10000/4096;
        
        //TIM_SetCompare1(TIM2,duty);
        
        DMA_ClearITPendingBit(DMA1_IT_TC1);
    }
}

uint16_t k1=1,k2=0,k3=0,k4=0;

void EXTI15_10_IRQHandler(void){
    
    if(EXTI_GetITStatus(EXTI_Line10)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_10)==Bit_RESET){
            //K1 被按下
            k1 = !k1;
        }
        EXTI_ClearITPendingBit(EXTI_Line10);
    }else if(EXTI_GetITStatus(EXTI_Line11)==SET){
        if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==Bit_RESET){
            //K2 被按下
            k2++;
        }else if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_11)==Bit_RESET){
            //K3 被按下
            k3++;
        }
        
        EXTI_ClearITPendingBit(EXTI_Line11);
    }
    else if(EXTI_GetITStatus(EXTI_Line12)==SET){
        if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_12)==Bit_RESET){
            //K4 被按下
            k4++;
        }
        EXTI_ClearITPendingBit(EXTI_Line12);
    }
    
}

#define MAX_RPS 23
const float pulse_per_rotation = 4 * 11 *9.27666; // 脉冲每转




/**
    读取编码器的位置增量（已处理编码器计数器溢出）
*/
int16_t read_encoder_delta_location(uint16_t raw_encoder_count) {
    static int16_t prev;
   
    int32_t delta = raw_encoder_count - prev;
    if (delta > 32768) delta -= 65536; // 正向溢出修正
    else if (delta < -32768) delta += 65536; // 反向溢出修正
    prev = raw_encoder_count;
    return delta;
}


void TIM4_IRQHandler(void){
    
    static uint8_t location_pid_count =0;
    
    if(TIM_GetITStatus(TIM4,TIM_IT_Update)){
        
        location_pid_count++;
        
        if(location_pid_count>=4){
            
            location_pid.Kp = get_rp_val(1)/4096.0 * 5;     
            location_pid.Ki = get_rp_val(2)/4096.0 * 5;
            location_pid.Kd = get_rp_val(3)/4096.0 * 5;

            
            location_pid_count=0;
            // 这里应该改为读取之后将其清零，这样就能每次都读取编码器的增量
            current_encoder_count = TIM_GetCounter(TIM3);
    
            location_pid.target = get_rp_val(0)/4096.0 *  pulse_per_rotation ;//- pulse_per_rotation;     

            static int16_t current_location=0;
            
            current_location += read_encoder_delta_location(current_encoder_count);
            
            location_pid.current = current_location;
            
            float target_delta_degree=0;
            if(k1){
                location_pid.last_error=0;
                location_pid.error_sum=0;
                target_delta_degree=0;
                current_location=location_pid.target ;
            }else{
            
                

                // 角度环目标值的变化量
                target_delta_degree = pid(&location_pid);
            
            }
            
            degree_pid.target = DEGREE_RP_CENTER - target_delta_degree;
        }
        

        


        degree_pid.current =  get_rp_val(4);
        
  
        if(degree_pid.current>3000||degree_pid.current<1000){
            // 关闭电机
            k1=1;
        }
        
        float out_duty;
        
        if(k1){
            degree_pid.last_error=0;
            degree_pid.error_sum=0;
            out_duty=0;
        }else{
            // 内环（角度环）pid
            out_duty = pid(&degree_pid);
        }
        
        int16_t current_duty = set_speed(out_duty,100);
        
       
        
        TIM_ClearITPendingBit(TIM4,TIM_IT_Update);
    }
}

